﻿/*
 * @文件描述: 电机基本驱动程序
 * @版本: 
 * @作者: 徐俊鑫
 * @Date: 2021-09-23 07:50:59
 */


using System;
using CMLCOMLib;
using System.Diagnostics;
using System.Threading;
using System.Text;
using static Motor_Control.COM_CODE;

namespace Motor_Control
{
    /// <summary>
    /// 电机连接状态枚举
    /// </summary>
    public enum MOTOR_CONNECTION
    {
        OK = 1,
        DISCONNECT = 0,
        CONNECTING = 2,

    }
    
    /// <summary>
    /// 手臂移动的枚举类型
    /// </summary>
    public enum HAND_DIRECTION
    {
        UP = 0,
        DOWN = 1,
        LEFT = 2,
        RIGHT = 3,
    }
    /// <summary>
    /// 电机id
    /// </summary>
    public enum MOTOR_ID
    {
        MOTOR_ID_1 = 1,
        MOTOR_ID_2 = 2,
    }
    /// <summary>
    /// 电机旋转方向
    /// </summary>
    public enum MOTOR_DIRECTION
    {
        LEFT = 0,
        RIGHT = 1,
    }
    /// <summary>
    /// 电机控制类,该类为单例模式,管理和电机有关的事务
    /// </summary>
    public class MotorControl
    {
        //定义一个静态变量来保存类的实例
        static MotorControl _Singleton;
        private static object Singleton_Lock = new object();
        AmpObj AmpX;
        AmpObj AmpY;
        //***************************************************
        //*
        //*  EtherCAT Network
        //*
        //***************************************************
        //// A negative node number refers to the drive's physical position on the network
        //// -1 for the first drive, -2 for the second, etc.
        //const int X_AXIS_ECAT_NODE = -1;
        //const int Y_AXIS_ECAT_NODE = -2;
        //EcatObj ecatObj;

        //***************************************************
        //
        //  CANOpen Network
        //
        //***************************************************
        public const short X_AXIS_CAN_ADDRESS = 1;
        public const short Y_AXIS_CAN_ADDRESS = 2;
        canOpenObj canOpen;
        HomeSettingsObj Home;
        ProfileSettingsObj ProfileSettings;
        MotorControlView view;
        MOTOR_CONNECTION connection = MOTOR_CONNECTION.DISCONNECT;
        Thread motorUpdate, connectMotorTrd;

        private MotorControl()
        {
            //motorInit();
            //注册接收回调函数
            InvokeCommand sendMotorPos = new InvokeCommand(REQUEST_CODE.SEND, TYPE_CODE.MOTOR_POS, this.socketCallback);
        }
        public static MotorControl CreateInstance()
        {
            if (_Singleton == null) //双if +lock
            {
                lock (Singleton_Lock)
                {
                    if (_Singleton == null)
                    {
                        _Singleton = new MotorControl();
                    }
                }
            }
            return _Singleton;
        }
        public MotorControl getInstance()
        {
            return this;
        }
        /// <summary>
        /// 绑定view对象
        /// </summary>
        /// <param name="view"> view对象</param>
        public void bindView(MotorControlView view)
        {
            this.view = view;

        }
        /// <summary>
        /// 处理来自对方的数据请求，不需要显式调用，由Analysis类自动处理并调用
        /// </summary>
        /// <param name="motorid"></param>
        private void socketCallback(object motorid)
        {
            StringBuilder stringBuilder = new StringBuilder();
            int value0 = int.Parse(motorid.ToString());
            //Console.WriteLine("ID:"+value0);
            double sendvalue0 = this.getPositionActual((MOTOR_ID)value0);
            stringBuilder = Analysis.formatString(REQUEST_CODE.SEND, TYPE_CODE.MOTOR_POS, 1);
            stringBuilder.Append("Value[0]=");
            stringBuilder.Append(sendvalue0.ToString());
            stringBuilder.Append(";");
            MySocket.Send(MySocket.getWorkingSocket(), stringBuilder.ToString());

        }
        /// <summary>
        /// 获取电机实际位置
        /// </summary>
        /// <param name="motor_ID"> 电机的ID，1或2</param>
        /// <returns></returns>
        public double getPositionActual(MOTOR_ID motor_ID)
        {
            // Random random=new Random();
            // return random.Next();
            switch ((int)motor_ID)
            {
                case MotorControl.X_AXIS_CAN_ADDRESS:
                    if (AmpX == null) return 0;

                    return AmpX.PositionActual;
                    break;
                case Y_AXIS_CAN_ADDRESS:
                    if (AmpY == null) return 0;
                    return AmpY.PositionActual;

                    break;

            }
            return 0;
        }
        /// <summary>
        /// 停止获取电机的位置
        /// </summary>
        /// <param name="motorID"> 电机ID</param>
        public void StopContinuousMotorPos(MOTOR_ID motorID)
        {

        }

        /// <summary>
        /// 控制电机相对转动
        /// </summary>
        /// <param name="motorID"> 电机ID</param>
        /// <param name="direction">电机转动的方向</param>
        /// <param name="distance">电机转动的距离</param>
        public void MotorControlRel(MOTOR_ID motorID, MOTOR_DIRECTION direction, int distance)
        {

        }
        /// <summary>
        /// 控制电机绝对转动
        /// </summary>
        /// <param name="motorID"> 电机ID</param>
        /// <param name="direction">电机转动的方向</param>
        /// <param name="distance">电机转动的距离</param>
        public void MotorControlAbs(MOTOR_ID motorID, MOTOR_DIRECTION direction, int distance)
        {

        }
        /// <summary>
        /// 立即停止电机转动
        /// </summary>
        /// <param name="motorID">电机ID</param>
        public void StopMotor(MOTOR_ID motorID)
        {

        }
        /// <summary>
        ///  控制手臂移动方向,是MotorControl的高阶版,直接输入上下左右的代码即可
        /// </summary>
        /// <param name="direction"></param>
        public void HandControl(HAND_DIRECTION direction, int distance)
        {
            motorControl((double)direction, distance);
        }
        /// <summary>
        /// 电机控制函数,每次调用会相对移动一段距离
        /// </summary>
        /// <param name="dir"> 旋转方向,分别为上下左右</param>
        /// <param name="distance">移动距离</param>
        public void motorControl(double dir, double distance)
        {
            try
            {
                switch (dir)
                {
                    case 0:
                        AmpX.MoveRel(distance);
                        Trace.WriteLine(dir);
                        break;
                    case 1:
                        AmpX.MoveRel(-distance);
                        Trace.WriteLine(dir);
                        break;
                    case 2:
                        AmpY.MoveRel(distance);
                        Trace.WriteLine(dir);
                        break;
                    case 3:
                        AmpY.MoveRel(-distance);
                        Trace.WriteLine(dir);
                        break;
                    default:
                        break;
                }
            }
            catch (Exception ex)
            {
                view.onViewDisplayError(ex);
            }
        }
        /// <summary>
        /// 执行home功能
        /// </summary>
        public void executeHome()
        {
            try
            {


                Home = AmpX.HomeSettings;
                Home.HomeVelFast = (AmpX.VelocityLoopSettings.VelLoopMaxVel) / 10;
                Home.HomeVelSlow = (AmpX.VelocityLoopSettings.VelLoopMaxVel) / 15;
                Home.HomeAccel = (AmpX.VelocityLoopSettings.VelLoopMaxAcc) / 10;
                Home.HomeMethod = CML_HOME_METHOD.CHOME_INDEX_POSITIVE;
                Home.HomeOffset = 0;
                AmpX.HomeSettings = Home;
                AmpX.GoHome();
                AmpX.WaitMoveDone(10000);


                Home = AmpY.HomeSettings;
                Home.HomeVelFast = (AmpY.VelocityLoopSettings.VelLoopMaxVel) / 10;
                Home.HomeVelSlow = (AmpY.VelocityLoopSettings.VelLoopMaxVel) / 15;
                Home.HomeAccel = AmpY.VelocityLoopSettings.VelLoopMaxAcc / 10;
                Home.HomeMethod = CML_HOME_METHOD.CHOME_NONE;

                Home.HomeOffset = 0;
                AmpY.HomeSettings = Home;
                AmpY.GoHome();
                AmpY.WaitMoveDone(10000);


            }
            catch (Exception ex)
            {
                view.onViewDisplayError(ex);
            }
        }
        /// <summary>
        /// 电机状态刷新线程
        /// </summary>
        private void motorUpdater()
        {
            while (connection == MOTOR_CONNECTION.OK)
            {
                view.onViewSetMotorPos(MOTOR_ID.MOTOR_ID_1, getPositionActual(MOTOR_ID.MOTOR_ID_1));
                view.onViewSetMotorPos(MOTOR_ID.MOTOR_ID_2, getPositionActual(MOTOR_ID.MOTOR_ID_2));
                Thread.Sleep(50);
            }
        }
        /// <summary>
        /// 连接电机函数
        /// </summary>
        private void connectMotor()
        {
            try
            {
                connection = MOTOR_CONNECTION.CONNECTING;
                view.onViewSetMotorConnection(connection);

                // initialization

                AmpX = new AmpObj();
                AmpY = new AmpObj();

                //***************************************************
                //
                //  CANOpen Network
                //
                //***************************************************
                canOpen = new canOpenObj();
                //
                //**************************************************************************
                //* The next two lines of code are optional. If no bit rate is specified,
                //* then the default bit rate (1 Mbit per second) is used.  If no port name
                //* is specified, then CMO will use the first supported CAN card found and
                //* use channel 0 of that card.
                //**************************************************************************
                // Set the bit rate to 1 Mbit per second
                //canOpen.BitRate = CML_BIT_RATES.BITRATE_1_Mbit_per_sec;
                // Indicate that channel 0 of a Copley CAN card should be used
                //canOpen.PortName = "copley0";
                //// Indicate that channel 0 of a KVaser card should be used
                //canOpen.PortName = "kvaser0";
                //// Indicate that channel 0 of an IXXAT card should be used
                //canOpen.PortName = "IXXAT0";
                //// Indicate that channel 0 of an IXXAT card (V3.x or newer drivers) should be used
                canOpen.PortName = "IXXATV30";
                //***************************************************
                //* Initialize the CAN card and network
                //***************************************************
                canOpen.Initialize();
                //***************************************************
                //* Initialize the amplifier
                //***************************************************
                AmpX.Initialize(canOpen, X_AXIS_CAN_ADDRESS);//AmpX初始化就报错了
                AmpY.Initialize(canOpen, Y_AXIS_CAN_ADDRESS);


                //***************************************************
                //*
                //*  EtherCAT Network
                //*
                //***************************************************
                //ecatObj = new EcatObj();
                //
                //***************************************************
                //* The next line of code is optional. The port name is the IP address of 
                //* the Ethernet adapter. Alternatively, a shortcut name “eth” can be used 
                //* in conjunction with the adapter number. For example “eth0” for the first 
                //* Ethernet adapter, “eth1” for the second adapter. If no port name is 
                //* supplied, it will default to “eth0”.
                //**************************************************************************
                //// Indicate that the first Ethernet adapter is to be used
                //ecatObj.PortName = "eth0";
                //// Specify an IP address
                //ecatObj.PortName = "192.168.1.1";
                //
                //***************************************************
                //* Initialize the EtherCAT network
                //***************************************************
                //ecatObj.Initialize();
                //
                //***************************************************
                //* Initialize the amplifier
                //***************************************************
                //AmpX.InitializeEcat(ecatObj, X_AXIS_ECAT_NODE);
                //AmpY.InitializeEcat(ecatObj, Y_AXIS_ECAT_NODE);
                //
                //***************************************************
                //* Initialize both axis of a 2 axis EtherCAT drive (AE2, XE2, etc)
                //***************************************************
                //// Initialize the first axis. 2 axis etherCAT drives are treated
                //// like a single node on the EtherCAT bus, so only one ecat node ID
                //// is used.
                //AmpX.InitializeEcat(ecatObj, X_AXIS_ECAT_NODE);
                //// Initalize the second axis by passing in the previously initialized
                //// x axis amp object as a paramter
                //AmpY.InitializeEcatSubAxis(AmpX);

                AmpX.HaltMode = CML_HALT_MODE.HALT_DECEL;
                AmpY.HaltMode = CML_HALT_MODE.HALT_DECEL;



                //set up profile settings for moves
                //为运动设置好“配置设置”。“配置设置”指的是用户自动义的一些参数。Profile的含义是一组对于用户进行描述的数据。
                ProfileSettings = AmpX.ProfileSettings; // read profile settings from amp
                ProfileSettings.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
                //The profile acceleration value that the motor uses when starting the move. 
                ProfileSettings.ProfileAccel = (AmpX.VelocityLoopSettings.VelLoopMaxAcc) / 10;
                ProfileSettings.ProfileDecel = (AmpX.VelocityLoopSettings.VelLoopMaxDec) / 10;
                ProfileSettings.ProfileVel = (AmpX.VelocityLoopSettings.VelLoopMaxVel) / 10;
                AmpX.ProfileSettings = ProfileSettings;

                ProfileSettings = AmpY.ProfileSettings; // read profile settings from amp
                ProfileSettings.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
                ProfileSettings.ProfileAccel = (AmpY.VelocityLoopSettings.VelLoopMaxAcc) / 10;
                ProfileSettings.ProfileDecel = (AmpY.VelocityLoopSettings.VelLoopMaxDec) / 10;
                ProfileSettings.ProfileVel = (AmpY.VelocityLoopSettings.VelLoopMaxVel) / 10;
                AmpY.ProfileSettings = ProfileSettings;
                connection = MOTOR_CONNECTION.OK;
                view.onViewSetMotorConnection(connection);

                motorUpdate = new Thread(motorUpdater);
                motorUpdate.Start();
            }
            catch (Exception ex)
            {
                connection = MOTOR_CONNECTION.DISCONNECT;
                view.onViewSetMotorConnection(connection);

                view.onViewDisplayError(ex);
            }
        }
        /// <summary>
        /// 初始化电机
        /// </summary>
        public void motorInit()
        {
            connectMotorTrd = new Thread(connectMotor);
            connectMotorTrd.Start();
        }
        /// <summary>
        /// 断开电机的连接
        /// </summary>
        public void disconnectMotor()
        {

        }
        /// <summary>
        /// 获取电机的连接状态
        /// </summary>
        /// <returns>MOTOR_CONNECTION 电机连接状态枚举</returns>
        public MOTOR_CONNECTION getMotorConnection()
        {

            return this.connection;

        }


    }


}
